In [1]:

    

import sympy
sympy.init_printing()


    

In [2]:

    

def derive_equations():
    a, b, c = sympy.symbols('a, b, c')
    c_x, c_y, c_z = sympy.symbols('c_x, c_y, c_z')
    x, y, z = sympy.symbols('x, y, z')

    eq = (x - c_x)**2/a + (y - c_y)**2/b + (z - c_z)**2/c
    
    sympy.Poly(eq.expand().simplify(), [x, y, z])
    
    c_vect = sympy.Matrix([ it.simplify() for it in sympy.Poly(eq.expand().simplify(), [x, y, z]).coeffs()])
    c_syms = sympy.Matrix(sympy.symbols('c_0:{:d}'.format(len(c_vect))))
    C = c_vect
    
    rev_coeffs = sympy.solve(list(sympy.Matrix(C) - sympy.Matrix(c_syms)),
        [a, b, c, c_x, c_y, c_z, c_syms[-1]], dict=True)[0]

    def X(n):
        x_vect = sympy.Matrix(sympy.symbols('x_0:{:d}'.format(n)))
        xsq_vect = x_vect.multiply_elementwise(x_vect)

        y_vect = sympy.Matrix(sympy.symbols('y_0:{:d}'.format(n)))
        ysq_vect = y_vect.multiply_elementwise(y_vect)

        z_vect = sympy.Matrix(sympy.symbols('z_0:{:d}'.format(n)))
        zsq_vect = z_vect.multiply_elementwise(z_vect)
        ones = sympy.ones(n, 1)
        X = xsq_vect.row_join(x_vect).row_join(ysq_vect).row_join(
            y_vect).row_join(zsq_vect).row_join(z_vect).row_join(ones)
        return sympy.Matrix(X)

    def Y(n):
        return sympy.ones(n, 1)

    def XTY(n):
        return X(n).T*Y(n)
    
    def XTX(n):
        return X(n).T*X(n)

    def dXTY(n):
        return X(n).T*Y(n) - X(n-1).T*Y(n-1)

    def dXTX(n):
        return X(n).T*X(n) - X(n-1).T*X(n-1)
    
    dXTY2 = dXTY(2)
    dXTX2 = dXTX(2)

    f_dXTX = sympy.lambdify(sympy.symbols('x_1, y_1, z_1'), dXTX(2))
    f_dXTY = sympy.lambdify(sympy.symbols('x_1, y_1, z_1'), dXTY(2))
    
    return locals()


    

In [3]:

    

eqs = derive_equations()
eqs['dXTY2']


    

    
Out[3]:





$$\left[\begin{matrix}x_{1}^{2}\\x_{1}\\y_{1}^{2}\\y_{1}\\z_{1}^{2}\\z_{1}\\1\end{matrix}\right]$$

In [4]:

    

eqs['c_vect']


    

    
Out[4]:





$$\left[\begin{matrix}\frac{1}{a}\\- \frac{2 c_{x}}{a}\\\frac{1}{b}\\- \frac{2 c_{y}}{b}\\\frac{1}{c}\\- \frac{2 c_{z}}{c}\\\frac{c_{z}^{2}}{c} + \frac{c_{y}^{2}}{b} + \frac{c_{x}^{2}}{a}\end{matrix}\right]$$

In [5]:

    

eqs['rev_coeffs']


    

    
Out[5]:





$$\left \{ a : \frac{1}{c_{0}}, \quad b : \frac{1}{c_{2}}, \quad c : \frac{1}{c_{4}}, \quad c_{6} : \frac{c_{5}^{2}}{4 c_{4}} + \frac{c_{3}^{2}}{4 c_{2}} + \frac{c_{1}^{2}}{4 c_{0}}, \quad c_{x} : - \frac{c_{1}}{2 c_{0}}, \quad c_{y} : - \frac{c_{3}}{2 c_{2}}, \quad c_{z} : - \frac{c_{5}}{2 c_{4}}\right \}$$

In [6]:

    

eqs['XTX'](2)


    

    
Out[6]:





$$\left[\begin{matrix}x_{0}^{4} + x_{1}^{4} & x_{0}^{3} + x_{1}^{3} & x_{0}^{2} y_{0}^{2} + x_{1}^{2} y_{1}^{2} & x_{0}^{2} y_{0} + x_{1}^{2} y_{1} & x_{0}^{2} z_{0}^{2} + x_{1}^{2} z_{1}^{2} & x_{0}^{2} z_{0} + x_{1}^{2} z_{1} & x_{0}^{2} + x_{1}^{2}\\x_{0}^{3} + x_{1}^{3} & x_{0}^{2} + x_{1}^{2} & x_{0} y_{0}^{2} + x_{1} y_{1}^{2} & x_{0} y_{0} + x_{1} y_{1} & x_{0} z_{0}^{2} + x_{1} z_{1}^{2} & x_{0} z_{0} + x_{1} z_{1} & x_{0} + x_{1}\\x_{0}^{2} y_{0}^{2} + x_{1}^{2} y_{1}^{2} & x_{0} y_{0}^{2} + x_{1} y_{1}^{2} & y_{0}^{4} + y_{1}^{4} & y_{0}^{3} + y_{1}^{3} & y_{0}^{2} z_{0}^{2} + y_{1}^{2} z_{1}^{2} & y_{0}^{2} z_{0} + y_{1}^{2} z_{1} & y_{0}^{2} + y_{1}^{2}\\x_{0}^{2} y_{0} + x_{1}^{2} y_{1} & x_{0} y_{0} + x_{1} y_{1} & y_{0}^{3} + y_{1}^{3} & y_{0}^{2} + y_{1}^{2} & y_{0} z_{0}^{2} + y_{1} z_{1}^{2} & y_{0} z_{0} + y_{1} z_{1} & y_{0} + y_{1}\\x_{0}^{2} z_{0}^{2} + x_{1}^{2} z_{1}^{2} & x_{0} z_{0}^{2} + x_{1} z_{1}^{2} & y_{0}^{2} z_{0}^{2} + y_{1}^{2} z_{1}^{2} & y_{0} z_{0}^{2} + y_{1} z_{1}^{2} & z_{0}^{4} + z_{1}^{4} & z_{0}^{3} + z_{1}^{3} & z_{0}^{2} + z_{1}^{2}\\x_{0}^{2} z_{0} + x_{1}^{2} z_{1} & x_{0} z_{0} + x_{1} z_{1} & y_{0}^{2} z_{0} + y_{1}^{2} z_{1} & y_{0} z_{0} + y_{1} z_{1} & z_{0}^{3} + z_{1}^{3} & z_{0}^{2} + z_{1}^{2} & z_{0} + z_{1}\\x_{0}^{2} + x_{1}^{2} & x_{0} + x_{1} & y_{0}^{2} + y_{1}^{2} & y_{0} + y_{1} & z_{0}^{2} + z_{1}^{2} & z_{0} + z_{1} & 2\end{matrix}\right]$$

In [7]:

    

eqs['C']


    

    
Out[7]:





$$\left[\begin{matrix}\frac{1}{a}\\- \frac{2 c_{x}}{a}\\\frac{1}{b}\\- \frac{2 c_{y}}{b}\\\frac{1}{c}\\- \frac{2 c_{z}}{c}\\\frac{c_{z}^{2}}{c} + \frac{c_{y}^{2}}{b} + \frac{c_{x}^{2}}{a}\end{matrix}\right]$$

In [8]:

    

%pylab inline
from mpl_toolkits.mplot3d import Axes3D

def test(eqs):

    data = []
    seed(1234)
    for i in range(1000):
        psi = 2*pi*rand()
        theta = pi*rand()
        data += [[sin(theta)*cos(psi), sin(theta)*np.sin(psi), np.cos(theta)]]
    
    #data = array(data)
    #fig = plt.figure()
    #ax = fig.add_subplot(111, projection='3d')
    #ax.plot3D(*data.T, '.')
    
    dxtx_val = np.zeros((7, 7))
    dxty_val = np.zeros((7, 1))

    for i in range(len(data)):
        p = data[i]
        dxtx_val += eqs['f_dXTX'](p[0], p[1], p[2])
        dxty_val += 0.1*eqs['f_dXTY'](p[0], p[1], p[2])
        
    res = np.linalg.inv(dxtx_val).dot(dxty_val)
    print('res', res)
    res_sub = { eqs['c_syms'][i]: res[i, 0] for i in range(6)}
    
    ans = { key: eqs['rev_coeffs'][key].subs(res_sub) for key in eqs['rev_coeffs'].keys() }
    
    print(eqs['c_vect'][-1].subs(ans))
    return locals()

res = test(eqs)
res['ans'], eqs['c_vect'][-1].subs(res['ans'])


    

    




Populating the interactive namespace from numpy and matplotlib
res [[ -2.48750000e+01]
 [ -2.10965898e-02]
 [ -2.57500000e+01]
 [ -2.06887673e-03]
 [ -2.52500000e+01]
 [ -1.21430643e-17]
 [  2.53750000e+01]]
-4.51458197220267e-6






    
Out[8]:





$$\left ( \left \{ a : -0.0402010050251256, \quad b : -0.0388349514563107, \quad c : -0.0396039603960396, \quad c_{6} : -4.51458197220267 \cdot 10^{-6}, \quad c_{x} : -0.000424052056057694, \quad c_{y} : -4.01723637384281 \cdot 10^{-5}, \quad c_{z} : -2.4045671944233 \cdot 10^{-19}\right \}, \quad -4.51458197220267 \cdot 10^{-6}\right )$$

In [9]:

    

eqs['c_vect'][-1].subs(res['ans'])


    

    
Out[9]:





$$-4.51458197220267 \cdot 10^{-6}$$

In [10]:

    

eqs['c_vect'][-1]


    

    
Out[10]:





$$\frac{c_{z}^{2}}{c} + \frac{c_{y}^{2}}{b} + \frac{c_{x}^{2}}{a}$$